/* Include files */
#include "math.h"
#include "mex.h"


/* Checks whether inputs are matrices */
#define IS_REAL_2D_FULL_DOUBLE(P) (!mxIsComplex(P) && \
mxGetNumberOfDimensions(P) == 2 && !mxIsSparse(P) && mxIsDouble(P))
#define IS_REAL_SCALAR(P) (IS_REAL_2D_FULL_DOUBLE(P) && mxGetNumberOfElements(P) == 1)

/*************************************************/
/* MAIN                                          */
/*************************************************/

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	/* Define output matrix */
	#define Afilled    plhs[0]
	#define Bfilled    plhs[1]

	/* Define input matrices */
	#define Amat 		prhs[0]
	#define Bmat    	prhs[1]
	#define FirmMatch 	prhs[2]
	#define Begin       prhs[3]
	#define Jm          prhs[4]

	int a, b, i, index, loc;

	/* Check the number of inputs is correct (Aoffer,Baccept,QbarA) */
	if(nrhs < 5)
		mexErrMsgTxt("Too few input arguments.");
	else if(nrhs > 5)
		mexErrMsgTxt("Too many input arguments.");

	if(!IS_REAL_2D_FULL_DOUBLE(Amat))
		mexErrMsgTxt("Amat must be real 2D full double array.");
	if(!IS_REAL_2D_FULL_DOUBLE(Bmat))
		mexErrMsgTxt("Bmat must be real 2D full double array.");
	if(!IS_REAL_2D_FULL_DOUBLE(FirmMatch))
		mexErrMsgTxt("FirmMatch must be real 2D full double array.");
	if(!IS_REAL_2D_FULL_DOUBLE(Begin))
		mexErrMsgTxt("Begin must be real 2D full double array.");
	if(!IS_REAL_2D_FULL_DOUBLE(Jm))
		mexErrMsgTxt("Jm must be real 2D full double array.");

	/* Dimensions and pointers for inputs */
	int n = mxGetN(FirmMatch);
	int N = mxGetM(Amat);

	int A = mxGetN(Amat);
	int B = mxGetN(Bmat);

	double *A_pt 		= mxGetPr(Amat);
	double *B_pt 		= mxGetPr(Bmat);
	double *Firm_pt 	= mxGetPr(FirmMatch);
	double *Begin_pt 	= mxGetPr(Begin);
	double *Jm_pt 		= mxGetPr(Jm);

	/* Output matrix */
	Afilled = mxCreateDoubleMatrix(n,A,mxREAL);
	double *Afill_pt = mxGetPr(Afilled);

	Bfilled = mxCreateDoubleMatrix(n,B,mxREAL);
	double *Bfill_pt = mxGetPr(Bfilled);

/***************************************************************/
/* Begin Filling       */
/***************************************************************/

	index = 0;
	loc = 0;
	for(i = 0; i < n; i++)
	{
		if(Firm_pt[i] > 0)
		{
			loc = Begin_pt[0] + i*Jm_pt[0] + Firm_pt[i] - 2;
	
			for(a = 0; a < A; a++)
				Afill_pt[index + n*a] = A_pt[loc + N*a];
			
			for(b = 0; b < B; b++)
				Bfill_pt[index + n*b] = B_pt[loc + N*b];
		}
		index++;
	}

}
